// Exemplos de como Utilizar Listas Simples Encadeadas em C++

#include "listas.h"

void Gera_Lista_Mapa(list<lista_mapa> *Lista1)
{
  // Variaveis
  lista_mapa aux_mapa;
  list<lista_mapa>::iterator im;

  // Inserindo os dados de cada Landmark na Lista_Mapas

  // Landmark 01 - Red
  aux_mapa.color = 1;
  aux_mapa.mx = 4;
  aux_mapa.my = 7;
  Lista1->push_back(aux_mapa);      

  // Landmark 02 - Blue
  aux_mapa.color = 2;
  aux_mapa.mx = -7;
  aux_mapa.my = -4;
  Lista1->push_back(aux_mapa);

  // Landmark 03 - Green
  aux_mapa.color = 3;
  aux_mapa.mx = -7;
  aux_mapa.my = 6;
  Lista1->push_back(aux_mapa);

  // Landmark 04 - Cyan
  aux_mapa.color = 4;
  aux_mapa.mx = -3;
  aux_mapa.my = 2;
  Lista1->push_back(aux_mapa);
 
  // Landmark 05 - Yellow
  aux_mapa.color = 5;
  aux_mapa.mx = 3;
  aux_mapa.my = -2;
  Lista1->push_back(aux_mapa);

  // Landmark 06 - Magenta
  aux_mapa.color = 6;
  aux_mapa.mx = 7;
  aux_mapa.my = -6;
  Lista1->push_back(aux_mapa);                                 
}

void Inicializa_Lista_Sensor(list<lista_sensor> *Lista2)
{
  // Variaveis
  lista_sensor aux_sensor;
  list<lista_sensor>::iterator is;
  int i;

  // Inicializando a Lista 02
  aux_sensor.dist = 0;
  aux_sensor.theta = 0;
  aux_sensor.flag = 0;

  for(i = 0; i<6; i++)
  {
    aux_sensor.color = i+1;
    Lista2->push_back(aux_sensor);
  }
}

void Atualiza_Lista_Sensor(list<lista_sensor> *Lista2, double dist_new[], double theta_new[], int signature[])
{
  // Variaveis
  list<lista_sensor>::iterator is;
  int i;

  is = Lista2->begin();
  for (i = 0; i<6; i++)
  {
    if (signature[i] == 1)
    {
      is->dist = dist_new[i];
      is->theta = theta_new[i];
      is->flag = 1;
    }
    
    if (signature[i] == 0)
    {
      is->flag = 0;
    }

    ++is;
  }
}

void Ler_Lista_Mapa(list<lista_mapa> *Lista1, int sig_flag, double pos_landmark[])
{
  // Variaveis
  list<lista_mapa>::iterator im;
  int i;
 
  // IS WE!
  im = Lista1->begin();
  for(i = 0; i<sig_flag; i++)
    ++im;
   
  // Recebendo os valores
  pos_landmark[0] = im->mx;		// Recebe a coordenada X do Landmark em relação ao Referencial Global
  pos_landmark[1] = im->my;		// Recebe a coordenada Y do Landmark em relação ao Referencial Global
}

void Ler_Lista_Sensor(list<lista_sensor> *Lista2, int sig_flag, double z_landmark[])
{
  // Variaveis
  list<lista_sensor>::iterator is;
  int i;
 
  // IS WE!
  is = Lista2->begin();
  for(i = 0; i<sig_flag; i++)
    ++is;
   
  // Recebendo os valores
  z_landmark[0] = is->dist;		// Recebe a distancia linear do Landmark em relação ao Referencial do Robo fornecido pelo Sensor
  z_landmark[1] = is->theta;		// Recebe a distancia angular do Landmark em relação ao Referencial do Robo fornecido pelo Sensor
}

